#!/usr/bin/env python

import rospy
import moveit_commander
import geometry_msgs.msg
import sys

def get_panda_arm_pose():
    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('get_panda_arm_pose', anonymous=True)

    robot = moveit_commander.RobotCommander()
    group_name = "panda_arm"
    move_group = moveit_commander.MoveGroupCommander(group_name)

    current_pose = move_group.get_current_pose().pose
    print(current_pose)
    px,py,pz = current_pose.position.x,current_pose.position.y,current_pose.position.z
    ox,oy,oz,ow = current_pose.orientation.x,current_pose.orientation.y,current_pose.orientation.z,current_pose.orientation.w
    return [px,py,pz],[ox,oy,oz,ow]

if __name__ == '__main__':
    try:
        current_pose = get_panda_arm_pose()
        print(current_pose)
    except rospy.ROSInterruptException:
        pass